%% Finding Trajectories for Helicopter Tour
% by ejc

clear all; close all;
initialize; eps = 0.0001; delta = 15; delta2 = 500; safety_thresh = 0;
% m = length(t);
m = length(t1);

% Toeplitz matrix for C1=C2=...=CN
top = toeplitz([1; zeros(N-2,1); -1], [1 -1 zeros(1,N-2)]);
T = zeros(m,m);
for i=1:(m/N)
   T((i-1)*N+1:i*N,(i-1)*N+1:i*N) = top; 
end

cvx_begin
    variables w(m) x(m) y(m) z(m) dx(n) dy(n) dz(n);
    minimize( sum(w) );
    
    subject to
        approxP = P + [dx dy dz];

        % Cone Form
        for i=1:m
            ind = int8(floor(((i-1)/N)+eps)+1);
            
            A = A1(i)-A2(i);
            B = B11(i)*approxP(ind,:) + B12(i)*approxP(ind+1,:) - B21(i)*approxP(ind,:) - B22(i)*approxP(ind+1,:);
            
            norm(A*[x(i) y(i) z(i)] + B) <= w(i);
        end
       
        % C1 continuity
        for i=1:(m/N)-1
            ind = (i-1)*N + 1;
            approxP(i+1,:) == (1/2)*([x(ind) y(ind) z(ind)] + [x(ind+N) y(ind+N) z(ind+N)]); 
        end
    
        % constraint that C1=C2=...=CN
        T*[x y z] == zeros(m,3);  
        
        % constraint on delta
        abs([dx dy dz]) <= delta2*ones(n,3);
        abs([dx(origIndex) dy(origIndex) dz(origIndex)]) <= delta*ones(size(origIndex,2),3);
%         for i=1:length(origIndex)
%             ind = origIndex(i);
%             [abs(dx(ind)) abs(dy(ind)) abs(dz(ind))] <= delta*ones(1,3);
%         end
        
        % constaint on z axis (temporary)
        z >= safety_thresh*ones(m,1); 
%         dz == zeros(n,1);
cvx_end



%% VISUALIZATION
% save De boor control points
Z = [];
for i=1:(m/N)
   C(i,:) = [x((i-1)*N + 1) y((i-1)*N + 1) z((i-1)*N + 1)];
   Z = [Z; approxP(i,:); C(i,:)];
end
Z = [Z; approxP(n,:)];

% % plot
plot3(P(:,1),P(:,2),P(:,3),'ro'); hold on;
plot3(approxP(:,1),approxP(:,2),approxP(:,3),'go'); hold on;
plot3(C(:,1),C(:,2),C(:,3),'b*'); hold on;
% plot3(Z(:,1),Z(:,2),Z(:,3), '-'); hold on; 

time = [0:0.1:1]';
for i=1:n-1
    F = ((1-time).^2)*approxP(i,:) + (2*(1-time).*time)*C(i,:) + (time.^2)*approxP(i+1,:);
    plot3(F(:,1),F(:,2),F(:,3)); hold on;
end
grid on; axis square;

% plot
% plot(P(:,1),P(:,2),'ro'); hold on;
% plot(approxP(:,1),approxP(:,2),'go'); hold on;
% plot(C(:,1),C(:,2),'b*'); hold on;
% plot(Z(:,1),Z(:,2), '-'); grid on; axis square;